

//-----------------------------------------------------------------------
QY_INLINE Plane::Plane( void )
{
	
}

//-----------------------------------------------------------------------
QY_INLINE Plane::Plane( const Plane &p )
: a( p.a ), b( p.b ), c( p.c ), d( p.d )
{
	
}

//-----------------------------------------------------------------------
QY_INLINE Plane::Plane( float va, float vb, float vc, float vd )
: a( va ), b( vb ), c( vc ), d( vd )
{
	
}

//-----------------------------------------------------------------------
QY_INLINE Plane::Plane( const Vector3 &normal, const float dist )
: a( normal.x ), b( normal.y ), c( normal.z ), d( -dist )
{
	
}

//-----------------------------------------------------------------------
QY_INLINE float Plane::operator [](const int index) const
{
	return ( &a )[index];
}

//-----------------------------------------------------------------------
QY_INLINE float & Plane::operator[]( const int index )
{
	return ( &a )[index];
}

//-----------------------------------------------------------------------
QY_INLINE Plane Plane::operator-() const
{
	return Plane( -a, -b, -c, -d );
}

//-----------------------------------------------------------------------
QY_INLINE Plane & Plane::operator=( const Plane &p )
{
	a = p.a; b = p.b; c = p.c; d = p.d;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE Plane Plane::operator+( const Plane &p ) const
{
	return Plane( a + p.a, b + p.b, c + p.c, d + p.d );
}

//-----------------------------------------------------------------------
QY_INLINE Plane Plane::operator-( const Plane &p ) const
{
	return Plane( a - p.a, b - p.b, c - p.c, d - p.d );
}

//-----------------------------------------------------------------------
QY_INLINE Plane & Plane::operator*=( const Matrix3 &m )
{
	Normal() *= m;
	return *this;
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::Compare( const Plane &p ) const
{
	return ( a == p.a && b == p.b && c == p.c && d == p.d );
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::Compare( const Plane &p, const float epsilon ) const
{
	if ( fabs( a - p.a ) > epsilon )
	{
		return false;
	}
	
	if ( fabs( b - p.a ) > epsilon )
	{
		return false;
	}
	
	if ( fabs( c - p.c ) > epsilon )
	{
		return false;
	}
	
	if ( fabs( d - p.d ) > epsilon )
	{
		return false;
	}
	
	return true;
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::Compare( const Plane &p, const float normalEpsilon, const float distEpsilon ) const
{
	if ( fabs( d - p.d ) > distEpsilon )
	{
		return false;
	}
	
	if ( fabs( a - p.a ) > normalEpsilon )
	{
		return false;
	}
	
	if ( fabs( b - p.a ) > normalEpsilon )
	{
		return false;
	}
	
	if ( fabs( c - p.c ) > normalEpsilon )
	{
		return false;
	}
	
	return true;
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::operator==( const Plane &p ) const
{
	return Compare( p );
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::operator!=( const Plane &p ) const
{
	return !Compare( p );
}

//-----------------------------------------------------------------------
QY_INLINE void Plane::Zero()
{
	a = b = c = d = 0.0f;
}

//-----------------------------------------------------------------------
QY_INLINE void Plane::SetNormal( const Vector3 &n )
{
	a = n.x;
	b = n.y;
	c = n.z;
}

//-----------------------------------------------------------------------
QY_INLINE const Vector3 & Plane::Normal() const
{
	return *(reinterpret_cast<const Vector3*> (&a));
}

//-----------------------------------------------------------------------
QY_INLINE Vector3 & Plane::Normal()
{
	return *(reinterpret_cast<Vector3*> (&a));
}

//-----------------------------------------------------------------------
QY_INLINE float Plane::Normalize()
{
	float length = Normal().Normalize();
	return length;
}

//-----------------------------------------------------------------------
QY_INLINE float Plane::Dist() const
{
	return -d;
}

//-----------------------------------------------------------------------
QY_INLINE void Plane::SetDist( const float dist )
{
	d = -dist;
}

//-----------------------------------------------------------------------
QY_INLINE void Plane::Set( float va, float vb, float vc, float vd )
{
	a = va; b = vb; c = vc; d = vd;
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::FromPoints( const Vector3 &p1, const Vector3 &p2, const Vector3 &p3 )
{
	Normal() = (p1 - p2).Cross( p3 - p2);
	if ( Normal().Normalize() == 0.0f )
	{
		return false;
	}
	d = - Normal() * p2;
	return true;
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::FromPointsSlow( const Vector3 &p0, const Vector3 &p1, const Vector3 &p2 )
{
	const Vector3 *p[3] = { &p0, &p1, &p2 };
	float l0 = ( p2 - p1 ).LengthSq();
	float l1 = ( p0 - p2 ).LengthSq();
	float l2 = ( p1 - p0 ).LengthSq();
	int index = TMax3Index( l0, l1, l2 );
	Vector3 v1 = *p[(index+1)%3] - *p[index];
	Vector3 v2 = *p[(index+2)%3] - *p[index];
	Normal() = v1.Cross( v2 );
	bool r = Normal().Normalize() != 0.0f;
	FitThroughPoint( p0 );
	return r;
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::FromVector( const Vector3 &dir1, const Vector3 &dir2, const Vector3 &pos )
{
	Normal() = dir1.Cross( dir2 );
	if ( Normal().Normalize() == 0.0f )
	{
		return false;
	}
	d = -(Normal()*pos);
	return true;
}

//-----------------------------------------------------------------------
QY_INLINE void Plane::FitThroughPoint( const Vector3 &p )
{
	d = -( Normal() * p );
}

//-----------------------------------------------------------------------
QY_INLINE Plane Plane::Translate( const Vector3 &trans ) const
{
	return Plane( a, b, c, d - trans * Normal() );
}

//-----------------------------------------------------------------------
QY_INLINE Plane & Plane::TranslateSelf( const Vector3 &trans )
{
	d -= trans * Normal();
	return *this;
}

//-----------------------------------------------------------------------
QY_INLINE Plane Plane::Rotate( const Vector3 &orig, const Matrix3 &rot ) const
{
	Plane p;
	p.Normal() = Normal() * rot;
	p.d = d + orig * Normal() - orig * p.Normal();
	return p;
}

//-----------------------------------------------------------------------
QY_INLINE Plane & Plane::RotateSelf( const Vector3 &orig, const Matrix3 &rot )
{
	d += orig * Normal();
	Normal() *= rot;
	d -= orig * Normal();
	return *this;
}

//-----------------------------------------------------------------------
QY_INLINE float Plane::DistanceFrom( const Vector3 &v ) const
{
	return a * v.x + b * v.y + c * v.z + d;
}

//-----------------------------------------------------------------------
QY_INLINE int Plane::Side( const Vector3 &v, const float epsilon /* = 0.0f */ ) const
{
	float dist = DistanceFrom( v );
	if ( dist > epsilon )
	{
		return PLANE_SIDE_FRONT;
	}
	if ( dist < -epsilon )
	{
		return PLANE_SIDE_BACK;
	}
	return PLANE_SIDE_ON;
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::LineIntersection( const Vector3 &start, const Vector3 &end, float &fraction ) const
{
	float d1, d2;
	
	d1 = Normal() * start + d;
	d2 = Normal() * end + d;
	if ( d1 == d2 ) 
	{
		return false;
	}
	if ( d1 > 0.0f && d2 > 0.0f ) 
	{
		return false;
	}
	if ( d1 < 0.0f && d2 < 0.0f )
	{
		return false;
	}
	fraction = ( d1 / ( d1 - d2 ) );
	return ( fraction >= 0.0f && fraction <= 1.0f );
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::LineIntersection( const Vector3 &start, const Vector3 &end ) const
{
	float d1, d2, fraction;
	
	d1 = Normal() * start + d;
	d2 = Normal() * end + d;
	if ( d1 == d2 ) 
	{
		return false;
	}
	if ( d1 > 0.0f && d2 > 0.0f ) 
	{
		return false;
	}
	if ( d1 < 0.0f && d2 < 0.0f ) 
	{
		return false;
	}
	fraction = ( d1 / ( d1 - d2 ) );
	return ( fraction >= 0.0f && fraction <= 1.0f );
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::RayIntersection( const Vector3 &start, const Vector3 &dir, float &scale ) const
{
	float d1, d2;
	
	d1 = Normal() * start + d;
	d2 = Normal() * dir;
	if ( d2 == 0.0f ) 
	{
		return false;
	}
	scale = -( d1 / d2 );
	return true;
}

//-----------------------------------------------------------------------
QY_INLINE const Vector4 &Plane::ToVec4() const
{
	return *reinterpret_cast<const Vector4 *>(&a);
}

//-----------------------------------------------------------------------
QY_INLINE Vector4 & Plane::ToVec4()
{
	return *reinterpret_cast<Vector4 *>(&a); 
}

//-----------------------------------------------------------------------
QY_INLINE const float * Plane::ToFloatPtr() const
{
	return reinterpret_cast<const float*>(&a);
}

//-----------------------------------------------------------------------
QY_INLINE float * Plane::ToFloatPtr()
{
	return reinterpret_cast<float *>(&a);
}

//-----------------------------------------------------------------------
QY_INLINE bool Plane::PlaneIntersection( const Plane &plane, Vector3 &start, Vector3 &dir ) const 
{
	double n00, n01, n11, det, invDet, f0, f1;
	
	n00 = Normal().LengthSq();
	n01 = Normal() * plane.Normal();
	n11 = plane.Normal().LengthSq();
	det = n00 * n11 - n01 * n01;
	
	if ( fabs(det) < 1e-6f ) {
		return false;
	}
	
	invDet = 1.0f / det;
	f0 = ( n01 * plane.d - n11 * d ) * invDet;
	f1 = ( n01 * d - n00 * plane.d ) * invDet;
	
	dir = Normal().Cross( plane.Normal() );
	start = (float)f0 * Normal() + (float)f1 * plane.Normal();
	return true;
}
